#ifndef ROTARY_ENCODER_HPP_
#define ROTARY_ENCODER_HPP_

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <errno.h>
#include <string.h>
#include <wiringPi.h>
#include <memory>

#include "rclcpp/rclcpp.hpp"

#define PIN1 0
#define PIN2 2

class RotaryEncoder
{
private:
    static uint8_t state;
    static long pos;
public:
    RotaryEncoder(/* args */);
    ~RotaryEncoder();
    int get_pos();
    static void pin_isr(void);
};

RotaryEncoder::RotaryEncoder(/* args */)
{
    if (wiringPiSetup() < 0) {
    //RCLCPP_FATAL(rclcpp::get_logger("ROTARY_ENCODER"), "wiringPiSetup");
    exit(EXIT_FAILURE);
    }

    pinMode (PIN1,  INPUT) ;
    pinMode (PIN2,  INPUT) ;
    pullUpDnControl(PIN1, PUD_UP);
    pullUpDnControl(PIN2, PUD_UP);

    //RCLCPP_INFO(rclcpp::get_logger("ROTARY_ENCODER", "Encoder init success.");

    if ( wiringPiISR (PIN1, INT_EDGE_BOTH, &pin_isr) < 0 ) {
       // RCLCPP_FATAL(rclcpp::get_logger("ROTARY_ENCODER"), "wiringPiISR");
        exit(EXIT_FAILURE);
    }
    
    if ( wiringPiISR (PIN2, INT_EDGE_BOTH, &pin_isr) < 0 ) {
       // RCLCPP_FATAL(rclcpp::get_logger("ROTARY_ENCODER"), "wiringPiISR");
        exit(EXIT_FAILURE);
    }
    RCLCPP_INFO(rclcpp::get_logger("ROTARY_ENCODER"), "Rotary encoder init successed.");
}

RotaryEncoder::~RotaryEncoder()
{
}

void RotaryEncoder::pin_isr(void)
{
    uint8_t p1val = digitalRead(PIN1);
    uint8_t p2val = digitalRead(PIN2);
    uint8_t s = state & 3;
    if (p1val) s |= 4;
    if (p2val) s |= 8;
    state = (s >> 2);
    
    switch (s) {
        case 1: case 7: case 8: case 14:
            pos++;
            return;
        case 2: case 4: case 11: case 13:
            pos--;
            return;
        case 3: case 12:
            pos += 2;
            return;
        case 6: case 9:
            pos -= 2;
            return;
    }
}

int RotaryEncoder::get_pos()
{
    return this->pos;
}


long RotaryEncoder::pos;
uint8_t RotaryEncoder::state;

#endif
